﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Brain.Logic3D.PathFinding
{
    public enum PathFinder3DStatus
    {
        Stopped, Searching, Finished, NoPath
    }
    public class PathFinder3D
    {
        public Map3D Map;

        public enum Direction
        {
            V1V2, V2V3, V1V3
        }

        public PathFinder3DStatus Status { get; protected set; }

        public List<PathNodeInfo3D> OpenList;
        public List<PathNodeInfo3D> WayList;
        public List<PathNode3D> UsedList;

        /// <summary>
        /// If the Path Finder should find the way in only 1
        /// Update call(it will use a While and will not stop
        /// until it find the path). Not recommended, this
        /// can slow down your game if the way is too big
        /// </summary>
        public bool CalculateAllAtOnce = false;

        /// <summary>
        /// If CalculateAllAtOnce is false, then this will be the TimeStep
        /// it will take to update again
        /// </summary>
        public int TimeStep = 32;

        /// <summary>
        /// How much Operations it should do in 1 update call
        /// </summary>
        public int OperationsPerUpdate = 1;

        public event SetVoid FoundWay;

        public float Height = 2;

        public PathFinder3D(Map3D map)
        {
            this.Map = map;

            OpenList = new List<PathNodeInfo3D>();
            WayList = new List<PathNodeInfo3D>();
            UsedList = new List<PathNode3D>();
        }

        private Vector3 startPoint;
        private PathNodeInfo3D startInfo;
        private Vector3 endPoint;
        public virtual void Search(PathNode3D start, Vector3 end)
        {
            LastNode = null;

            startPoint = start.Triangle.GetMidPoint();
            startInfo = new PathNodeInfo3D();
            startInfo.Position = startPoint;
            startInfo.ParentNode3D = start;

            this.endPoint = end;

            this.WayList.Clear();
            this.WayList.Capacity = 0;
            this.OpenList.Clear();
            this.OpenList.Capacity = 0;
            this.UsedList.Clear();
            this.UsedList.Capacity = 0;

            this.Status = PathFinder3DStatus.Searching;
        }

        public virtual void Stop()
        {
            this.endPoint = Vector3.Zero;
            this.startInfo = null;
            this.startPoint = Vector3.Zero;

            this.WayList.Clear();
            this.WayList.Capacity = 0;
            this.OpenList.Clear();
            this.OpenList.Capacity = 0;
            this.UsedList.Clear();
            this.UsedList.Capacity = 0;

            this.Status = PathFinder3DStatus.Stopped;
        }

        internal int timer = 0;
        public virtual void Update()
        {
            if (CalculateAllAtOnce)
            {
                while (this.Status == PathFinder3DStatus.Searching)
                {
                    Search();
                }
            }
            else
            {
                timer += Engine.Instance.GameTime.ElapsedGameTime.Milliseconds;
                if (timer > this.TimeStep)
                {
                    timer = 0;
                    if (this.Status == PathFinder3DStatus.Searching)
                    {
                        int op = OperationsPerUpdate;
                        while (op > 0)
                        {
                            Search();
                            op--;
                        }
                    }
                }
            }
        }

        public PathNodeInfo3D LastNode;
        protected virtual void Search()
        {
            FindNode(Direction.V1V2);
            if (Status == PathFinder3DStatus.Finished)
            {
                return;
            }
            FindNode(Direction.V2V3);
            if (Status == PathFinder3DStatus.Finished)
            {
                return;
            }
            FindNode(Direction.V1V3);
            if (Status == PathFinder3DStatus.Finished)
            {
                return;
            }

            float Minimum = float.MaxValue;
            int Index = -1;
            PathNodeInfo3D Node = null;
            for (int i = 0; i < OpenList.Count; i++)
            {
                PathNodeInfo3D node = OpenList[i];
                if (node.F < Minimum)
                {
                    Minimum = node.F;
                    Index = i;
                    Node = node;
                }
            }
            if (Index != -1)
            {
                LastNode = Node;
                OpenList.RemoveAt(Index);
            }
            else
            {
                Status = PathFinder3DStatus.NoPath;
            }
        }

        public float SafeDistance = 5;

        protected virtual void FindNode(Direction direction)
        {
            // Search for the Parent of this Node we are going to make now
            PathNodeInfo3D parent = null;
            if (LastNode == null)
            {
                parent = startInfo;
            }
            else
            {
                parent = LastNode;
            }

            if (parent.ParentNode3D == null)
            {
                // Parent is Null/out of a triangle
                return;
            }

            PathNode3D Triangle = null;
            switch (direction)
            {
                case Direction.V1V2:
                    {
                        Triangle = parent.ParentNode3D.PathV1V2;
                    }
                    break;
                case Direction.V2V3:
                    {
                        Triangle = parent.ParentNode3D.PathV2V3;
                    }
                    break;
                case Direction.V1V3:
                    {
                        Triangle = parent.ParentNode3D.PathV1V3;
                    }
                    break;
            }

            if (Triangle == null || UsedList.Contains(Triangle))
            {
                return;
            }
            else
            {
                UsedList.Add(Triangle);
            }

            // Makes a new node info
            Vector3 pos = parent.ParentNode3D.Triangle.GetMidPoint();
            Triangle t = parent.ParentNode3D.Triangle;

            float H = Vector3.Distance(pos, endPoint);

            BoundingBox box;
            Vector3 v2 = t.Vertex2 + new Vector3(0, Height, 0);
            GetTriangleBoundingBox(ref t.Vertex1, ref v2, ref t.Vertex3, out box);

            if (box.Contains(endPoint) == ContainmentType.Contains)
            {
                Status = PathFinder3DStatus.Finished;
                FindParentAndWay(parent);
                if (FoundWay != null)
                {
                    FoundWay();
                }
                return;
            }

            PathNodeInfo3D node = new PathNodeInfo3D(Triangle, parent);
            node.Position = pos;
            float G = Vector3.Distance(node.Position, startPoint);
            node.F = G + H;

            this.OpenList.Add(node);
        }
        protected virtual void FindParentAndWay(PathNodeInfo3D info)
        {
            if (info != null)
            {
                WayList.Add(info);
                FindParentAndWay(info.Parent);
            }
        }

        /// <summary>
        /// Computes the bounding box of three points. - From BEPU
        /// </summary>
        /// <param name="a">First vertex of the triangle.</param>
        /// <param name="b">Second vertex of the triangle.</param>
        /// <param name="c">Third vertex of the triangle.</param>
        /// <param name="aabb">Bounding box of the triangle.</param>
        public static void GetTriangleBoundingBox(ref Vector3 a, ref Vector3 b, ref Vector3 c, out BoundingBox aabb)
        {
#if !WINDOWS
            aabb = new BoundingBox();
#endif
            //X axis
            if (a.X > b.X && a.X > c.X)
            {
                //A is max
                aabb.Max.X = a.X;
                if (b.X > c.X)
                    aabb.Min.X = c.X; //C is min
                else
                    aabb.Min.X = b.X; //B is min
            }
            else if (b.X > c.X)
            {
                //B is max
                aabb.Max.X = b.X;
                if (a.X > c.X)
                    aabb.Min.X = c.X; //C is min
                else
                    aabb.Min.X = a.X; //A is min
            }
            else
            {
                //C is max
                aabb.Max.X = c.X;
                if (a.X > b.X)
                    aabb.Min.X = b.X; //B is min
                else
                    aabb.Min.X = a.X; //A is min
            }
            //Y axis
            if (a.Y > b.Y && a.Y > c.Y)
            {
                //A is max
                aabb.Max.Y = a.Y;
                if (b.Y > c.Y)
                    aabb.Min.Y = c.Y; //C is min
                else
                    aabb.Min.Y = b.Y; //B is min
            }
            else if (b.Y > c.Y)
            {
                //B is max
                aabb.Max.Y = b.Y;
                if (a.Y > c.Y)
                    aabb.Min.Y = c.Y; //C is min
                else
                    aabb.Min.Y = a.Y; //A is min
            }
            else
            {
                //C is max
                aabb.Max.Y = c.Y;
                if (a.Y > b.Y)
                    aabb.Min.Y = b.Y; //B is min
                else
                    aabb.Min.Y = a.Y; //A is min
            }
            //Z axis
            if (a.Z > b.Z && a.Z > c.Z)
            {
                //A is max
                aabb.Max.Z = a.Z;
                if (b.Z > c.Z)
                    aabb.Min.Z = c.Z; //C is min
                else
                    aabb.Min.Z = b.Z; //B is min
            }
            else if (b.Z > c.Z)
            {
                //B is max
                aabb.Max.Z = b.Z;
                if (a.Z > c.Z)
                    aabb.Min.Z = c.Z; //C is min
                else
                    aabb.Min.Z = a.Z; //A is min
            }
            else
            {
                //C is max
                aabb.Max.Z = c.Z;
                if (a.Z > b.Z)
                    aabb.Min.Z = b.Z; //B is min
                else
                    aabb.Min.Z = a.Z; //A is min
            }
        }

        /*public Vector3 GetIntersection(Vector3 l1p1, Vector3 l1p2, Vector3 l2p1, Vector3 l2p2)
        {
            Vector3 pa = Vector3.Zero;
            Vector3 pb = Vector3.Zero;
            float mua = 0;
            float mub = 0;
            bool b = LineLineIntersect(l1p1, l1p2, l2p1, l2p2, ref pa, ref pb, ref mua, ref mub);
            return pa;
        }

        protected bool LineLineIntersect(
            Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4, ref Vector3 pa, ref Vector3 pb,
            ref float mua, ref float mub)
        {
            Vector3 p13, p43, p21;
            float d1343, d4321, d1321, d4343, d2121;
            float numer, denom;

            p13.X = p1.X - p3.X;
            p13.Y = p1.Y - p3.Y;
            p13.Z = p1.Z - p3.Z;
            p43.X = p4.X - p3.X;
            p43.Y = p4.Y - p3.Y;
            p43.Z = p4.Z - p3.Z;
            if (Math.Abs(p43.X) < float.Epsilon && Math.Abs(p43.Y) < float.Epsilon && Math.Abs(p43.Z) < float.Epsilon)
                return false;
            p21.X = p2.X - p1.X;
            p21.Y = p2.Y - p1.Y;
            p21.Z = p2.Z - p1.Z;
            if (Math.Abs(p21.X) < float.Epsilon && Math.Abs(p21.Y) < float.Epsilon && Math.Abs(p21.Z) < float.Epsilon)
                return false;

            d1343 = p13.X * p43.X + p13.Y * p43.Y + p13.Z * p43.Z;
            d4321 = p43.X * p21.X + p43.Y * p21.Y + p43.Z * p21.Z;
            d1321 = p13.X * p21.X + p13.Y * p21.Y + p13.Z * p21.Z;
            d4343 = p43.X * p43.X + p43.Y * p43.Y + p43.Z * p43.Z;
            d2121 = p21.X * p21.X + p21.Y * p21.Y + p21.Z * p21.Z;

            denom = d2121 * d4343 - d4321 * d4321;
            if (Math.Abs(denom) < float.Epsilon)
                return false;
            numer = d1343 * d4321 - d1321 * d4343;

            mua = numer / denom;
            mub = (d1343 + d4321 * (mua)) / d4343;

            pa.X = p1.X + mua * p21.X;
            pa.Y = p1.Y + mua * p21.Y;
            pa.Z = p1.Z + mua * p21.Z;
            pb.X = p3.X + mub * p43.X;
            pb.Y = p3.Y + mub * p43.Y;
            pb.Z = p3.Z + mub * p43.Z;

            return true;
        }*/


    }
}
